{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "60d186d5",
   "metadata": {},
   "source": [
    "# 三轴示教\n",
    "“具身稚能”[小米微电机Cybergear上手开发](https://space.bilibili.com/1969563580/channel/collectiondetail?sid=1683510) 《[两千元打造拖动示教无刷伺服机械臂](https://www.bilibili.com/video/BV1vu411T7d4/)》 视频代码"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a8708012",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-10-16T01:37:13.737231Z",
     "start_time": "2023-10-16T01:37:13.630816Z"
    }
   },
   "outputs": [],
   "source": [
    "import os\n",
    "import sys\n",
    "# 添加pcan_cybergear库的路径\n",
    "sys.path.append(os.path.join(\"..\", \"cybergear\"))\n",
    "\n",
    "from pcan_cybergear import CANMotorController\n",
    "import can\n",
    "import logging\n",
    "import time\n",
    "# Initialize logging\n",
    "logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(levelname)s - %(message)s')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bcee4222",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-10-16T01:37:14.042101Z",
     "start_time": "2023-10-16T01:37:13.999541Z"
    }
   },
   "outputs": [],
   "source": [
    "# Connect to the CAN bus with 1 Mbit/s bitrate\n",
    "bus = can.interface.Bus(bustype=\"pcan\", channel=\"PCAN_USBBUS1\", bitrate=1000000)\n",
    "motor1 = CANMotorController(bus, motor_id=101, main_can_id=254)\n",
    "motor2 = CANMotorController(bus, motor_id=102, main_can_id=254)\n",
    "motor3 = CANMotorController(bus, motor_id=103, main_can_id=254)\n",
    "\n",
    "motors = [motor1, motor2, motor3]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2029c460",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-10-16T01:39:18.578344Z",
     "start_time": "2023-10-16T01:39:18.527191Z"
    }
   },
   "outputs": [],
   "source": [
    "motor1.write_param_table(\"limit_cur\", 0.5)\n",
    "motor1.write_param_table(\"loc_kp\", 8)\n",
    "motor1.write_param_table(\"spd_kp\", 2)\n",
    "motor1.write_param_table(\"spd_ki\", 0.03)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b91bd39f",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-10-16T01:37:15.081262Z",
     "start_time": "2023-10-16T01:37:15.031995Z"
    }
   },
   "outputs": [],
   "source": [
    "motor2.write_param_table(\"limit_cur\", 2)\n",
    "motor2.write_param_table(\"loc_kp\", 8)\n",
    "motor2.write_param_table(\"spd_kp\", 2)\n",
    "motor2.write_param_table(\"spd_ki\", 0.03)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5c128222",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-10-16T01:40:10.921287Z",
     "start_time": "2023-10-16T01:40:10.870840Z"
    }
   },
   "outputs": [],
   "source": [
    "motor3.write_param_table(\"limit_cur\", 1)\n",
    "motor3.write_param_table(\"loc_kp\", 8)\n",
    "motor3.write_param_table(\"spd_kp\", 2)\n",
    "motor3.write_param_table(\"spd_ki\", 0.02)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "86cb5366",
   "metadata": {},
   "source": [
    "## 初始化"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d0ea5625",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-10-16T02:37:30.914238Z",
     "start_time": "2023-10-16T02:37:30.757054Z"
    }
   },
   "outputs": [],
   "source": [
    "for motor in motors:\n",
    "    motor.disable()\n",
    "    motor.set_0_pos()\n",
    "    motor.set_run_mode(motor.RunModes.POSITION_MODE) # 位置模式\n",
    "    motor.write_single_param(\"loc_ref\", value=0) # 目标位置\n",
    "\n",
    "motor1.write_single_param(\"limit_spd\", value=10) # 最大速度 rad/s\n",
    "motor2.write_single_param(\"limit_spd\", value=1.5) # 最大速度 rad/s\n",
    "motor3.write_single_param(\"limit_spd\", value=10) # 最大速度 rad/s"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a50f5b9d",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-10-16T02:37:33.909958Z",
     "start_time": "2023-10-16T02:37:33.901193Z"
    }
   },
   "outputs": [],
   "source": [
    "for motor in motors:\n",
    "    motor.enable()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a07fba09",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-10-16T02:00:55.778742Z",
     "start_time": "2023-10-16T02:00:52.735034Z"
    }
   },
   "outputs": [],
   "source": [
    "import time\n",
    "motor2.write_single_param(\"loc_ref\", value=-0.5)\n",
    "time.sleep(1.5)\n",
    "motor2.write_single_param(\"loc_ref\", value=0.5)\n",
    "time.sleep(1.5)\n",
    "motor2.write_single_param(\"loc_ref\", value=0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fd6fd4da",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-10-16T01:49:46.240202Z",
     "start_time": "2023-10-16T01:49:46.213183Z"
    }
   },
   "outputs": [],
   "source": [
    "motor3.set_motor_position_control(limit_spd=10, loc_ref=1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "98831b4d",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-10-16T02:37:35.814231Z",
     "start_time": "2023-10-16T02:37:35.765862Z"
    }
   },
   "outputs": [],
   "source": [
    "for motor in motors:\n",
    "    motor.disable()\n",
    "    motor.set_0_pos()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ef79902b",
   "metadata": {},
   "source": [
    "# 示教"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "26ebdb57",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-10-16T02:36:20.041579Z",
     "start_time": "2023-10-16T02:36:20.037579Z"
    }
   },
   "outputs": [],
   "source": [
    "import csv\n",
    "import time\n",
    "\n",
    "# 保存点位信息到CSV文件\n",
    "def save_positions_to_csv(positions, filename=\"positions.csv\"):\n",
    "    with open(filename, mode='w', newline='') as file:\n",
    "        writer = csv.writer(file)\n",
    "        writer.writerow([\"motor1\", \"motor2\", \"motor3\"])\n",
    "        for position in positions:\n",
    "            writer.writerow([position[0], position[1], position[2]])\n",
    "\n",
    "# 从CSV文件加载点位信息\n",
    "def load_positions_from_csv(filename=\"positions.csv\"):\n",
    "    positions = []\n",
    "    with open(filename, mode='r') as file:\n",
    "        csv_reader = csv.reader(file)\n",
    "        next(csv_reader)  # skip header\n",
    "        for row in csv_reader:\n",
    "            positions.append([float(row[0]), float(row[1]), float(row[2])])\n",
    "    return positions\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e8a5262b",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-10-16T03:43:35.750990Z",
     "start_time": "2023-10-16T03:42:58.969798Z"
    }
   },
   "outputs": [],
   "source": [
    "positions = []\n",
    "\n",
    "while True:\n",
    "    action = input(\"Press Enter to record the current position, or 'x' to exit: \")\n",
    "\n",
    "    if action == \"x\":\n",
    "        break\n",
    "    _, pos1, _ = motor1.write_single_param(\"loc_ref\", value=0)\n",
    "    _, pos2, _ = motor2.write_single_param(\"loc_ref\", value=0)\n",
    "    _, pos3, _ = motor3.write_single_param(\"loc_ref\", value=0)\n",
    "    pos = [pos1, pos2, pos3]\n",
    "    logging.info(pos)\n",
    "    positions.append(pos)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "11f43926",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-10-16T03:43:39.767633Z",
     "start_time": "2023-10-16T03:43:39.762595Z"
    }
   },
   "outputs": [],
   "source": [
    "len(positions)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "78f82f7b",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-10-16T03:43:41.197618Z",
     "start_time": "2023-10-16T03:43:41.194617Z"
    }
   },
   "outputs": [],
   "source": [
    "save_positions_to_csv(positions)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cd465e31",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-10-16T03:43:42.168151Z",
     "start_time": "2023-10-16T03:43:42.155149Z"
    }
   },
   "outputs": [],
   "source": [
    "import pandas as pd\n",
    "pd.read_csv(\"positions.csv\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4c39efdc",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-10-16T03:43:45.462783Z",
     "start_time": "2023-10-16T03:43:45.417683Z"
    }
   },
   "outputs": [],
   "source": [
    "for motor in motors:\n",
    "    motor.enable()\n",
    "    motor.write_single_param(\"loc_ref\", value=0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "eae8f95c",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-10-16T03:44:04.775243Z",
     "start_time": "2023-10-16T03:43:47.795998Z"
    }
   },
   "outputs": [],
   "source": [
    "loaded_positions = load_positions_from_csv()\n",
    "\n",
    "for position in loaded_positions:\n",
    "    logging.info(position)\n",
    "    motor1.write_single_param(\"loc_ref\", value=position[0])\n",
    "    motor2.write_single_param(\"loc_ref\", value=position[1])\n",
    "    motor3.write_single_param(\"loc_ref\", value=position[2])\n",
    "\n",
    "    time.sleep(1.5)  # 等待电机移动到目标位置\n",
    "\n",
    "motor1.write_single_param(\"loc_ref\", value=0)\n",
    "motor2.write_single_param(\"loc_ref\", value=0)\n",
    "motor3.write_single_param(\"loc_ref\", value=0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7ccbdbd2",
   "metadata": {
    "ExecuteTime": {
     "end_time": "2023-10-16T03:44:13.464028Z",
     "start_time": "2023-10-16T03:44:13.454022Z"
    }
   },
   "outputs": [],
   "source": [
    "for motor in motors:\n",
    "    motor.disable()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c27bca78",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
